#!/usr/bin/env python3

import numpy as np
import matplotlib.pyplot as plt
import math
import pdb

def line_angle(a, b, c, d):
    """计算两条线段之间的夹角"""
    # 计算向量AB和CD
    ab = (b[0] - a[0], b[1] - a[1])
    cd = (d[0] - c[0], d[1] - c[1])

    # 计算向量AB和CD的内积和模长
    ab_dot_cd = ab[0] * cd[0] + ab[1] * cd[1]
    ab_len = math.sqrt(ab[0] ** 2 + ab[1] ** 2)
    cd_len = math.sqrt(cd[0] ** 2 + cd[1] ** 2)

    # 计算夹角的余弦值
    cos_theta = ab_dot_cd / (ab_len * cd_len)

    # 将余弦值转换为角度
    theta = math.degrees(math.acos(cos_theta))

    return theta

def line_intersection(a, b, c, d):
    """计算两条线段的交点坐标"""
    # 计算直线AB和CD的参数a和b
    if b[0] - a[0] != 0:
        a1 = (b[1] - a[1]) / (b[0] - a[0])
        b1 = a[1] - a1 * a[0]
    else:
        a1 = None
        b1 = a[0]

    if d[0] - c[0] != 0:
        a2 = (d[1] - c[1]) / (d[0] - c[0])
        b2 = c[1] - a2 * c[0]
    else:
        a2 = None
        b2 = c[0]

    # 判断是否存在交点
    if a1 == a2:
        # 两条线段平行
        return None
    else:
        # 求解交点坐标
        if a1 is None:
            x = b1
            y = a2 * x + b2
        elif a2 is None:
            x = b2
            y = a1 * x + b1
        else:
            x = (b2 - b1) / (a1 - a2)
            y = a1 * x + b1
        intersection_point = (x, y)

        return intersection_point

def cubic_bezier_curve(p0, p1, p2, p3):
    """
    生成一条三次贝塞尔曲线，控制点分别为p1和p2，起点为p0，终点为p3。
    """
    t = np.linspace(0, 1, 100)
    b = np.zeros((100, 2))
    for i in range(100):
        b[i] = (1-t[i])**3*p0 + 3*t[i]*(1-t[i])**2*p1 + 3*t[i]**2*(1-t[i])*p2 + t[i]**3*p3
    return b

def quadratic_bezier_curve(p0, p1, p2):
    """
    生成一条二阶贝塞尔曲线，控制点为p1，起点为p0，终点为p2
    """
    # 将点转换为NumPy数组
    p0 = np.array(p0)
    p1 = np.array(p1)
    p2 = np.array(p2)
    
    # 定义参数t的范围
    t = np.linspace(0, 1, 100)
    t = t.reshape(100, 1)  # 调整 t 的形状以便与点数组 p 广播
    
    # 计算二阶贝塞尔曲线
    B = (1 - t) ** 2 * p0 + 2 * (1 - t) * t * p1 + t ** 2 * p2
    
    # 返回曲线上的点
    return B

# 测试代码
a = np.array([0, 0])
b = np.array([3, 3])
c = np.array([9, 5])
d = np.array([10, 0])

# 计算两条线段的交点
angle = line_angle(a, b, c, d)
print("angle: " , angle)
if 0 == angle:
    curve_points = np.array([[2, 5], [9, 5]])
    intersection_point = c
else:
    intersection_point = line_intersection(a, b, c, d)
    curve_points = quadratic_bezier_curve(a, intersection_point, d)
print("intersection_point:", intersection_point)
print("curve_points size:", len(curve_points))
print("curve_points:", curve_points)

plt.plot([intersection_point[0]], [intersection_point[1]], 'o', color='red')
plt.plot([a[0], b[0]], [a[1], b[1]], '-o')
plt.plot([c[0], d[0]], [c[1], d[1]], '-o')
plt.plot([b[0], intersection_point[0]], [b[1], intersection_point[1]], linestyle='--', color='blue')
plt.plot([intersection_point[0], c[0]], [intersection_point[1], c[1]], linestyle='--', color='yellow')
plt.plot(curve_points[:, 0], curve_points[:, 1], color='green')
plt.legend(['ab', 'cd', 'curve'])
plt.show()
